#ifndef _MOTOR_H_
#define _MOTOR_H_

/**
 *
 * @class Motor
 * @brief Class to control the motors of the robot
 * @author Nathan vdv
 *
 */

#include <string>
#include "serial.h"
#include "log.h"

/********************************************//**
 * @def Default serial port parameters
 *
 * @brief
 * Dit zijn de default parameters voor de serial port.
 *
 ***********************************************/
#define DEVICE "/dev/ttyUSB3"//ttys1
#define BAUDRATE 19200

/********************************************//**
 * @def T-Rex codes
 *
 * @brief
 * Hier krijgen alle codes van het T-Rex bordje een logische naam dmv defines
 *
 ***********************************************/
#define M1_FORWARD 0xC6
#define M1_BRAKE_LOW_FWD 0xC7
#define M1_REVERSE 0xC5
#define M1_BRAKE_LOW_RVS 0xC4
#define M1_ACCEL 99999

#define M2_FORWARD 0xCE
#define M2_BRAKE_LOW_FWD 0xCF
#define M2_REVERSE 0xCD
#define M2_BRAKE_LOW_RVS 0xCC
#define M2_ACCEL 99999

#define FORWARD 0xD6
#define REVERSE 0xD9
#define FORWARD_IND 0xD6
#define REVERSE_IND 0xD9

#define TURN_CW 0xD5
#define TURN_CCW 0xDA

using namespace std;

class Motor {

public:
    /********************************************//**
     * Constructor
     ***********************************************/
	Motor();
	/********************************************//**
     * Destructor
     ***********************************************/
	~Motor();

	void setSpeed(bool direction, int speed);
	void drive_individual(bool direction, int speed1, int speed2);
	void rotate(bool clockwise, int speed);
	void drive_left(bool direction, int speed);
	void drive_right(bool direction, int speed);
	void brake_left(bool direction, int speed);
	void brake_right(bool direction, int speed);
	void stop();

private:
    Log * logger;
    Serial * port_;
	bool sendCommand(unsigned char code, float data1, float data2);
};

#endif //_MOTOR_H_

